#include "RobotPayload.h"

namespace Team675
{

RobotPayload::RobotPayload(ControlMode _controlMode, TrackingMode _trackingMode, ShootingMode _shootingMode)
{
	m_controlMode = _controlMode;
	m_trackingMode = _trackingMode;
	m_shootingMode = _shootingMode;
	
	m_kinect = new KinectStick(1);
	
	m_leftStick = new Joystick(PAYLOAD_STICK_1);
	m_rightStick = new Joystick(PAYLOAD_STICK_2);
	if(IO_CHIP_ATTACHED) 
		m_enhancedIO = &DriverStation::GetInstance()->GetEnhancedIO();
	else 
		m_enhancedIO = NULL;
	
	m_confidenceThreshold = CONFIDENCE_THRESHOLD_DEFAULT;
	m_thetaSetpoint = 0.0;
	m_phiSetpoint = 0.0;
	m_speedSetpoint = 0.0;
	m_targetSelection = Target::TARGET_NONE;
}

void RobotPayload::Update()
{
	float t_thetaMotorValue, t_phiMotorValue, t_speedMotorValue,
		t_thetaSetpointChange, t_phiSetpointChange, t_speedSetpointChange;
	bool t_controlPickup, t_controlShoot, t_controlDump;
	
	switch(m_controlMode)
	{
	case CONTROL_AUTONOMOUS:
		// Autonomous Control without Kinect
		m_trackingMode = TRACKING_AUTONOMOUS;
		m_targetSelection = Target::TARGET_TOP;
		
		t_controlShoot = true;
		t_controlPickup = true;
		t_controlDump = false;
	break;
	
	case CONTROL_KINECT:
		// Autonomous Control with Kinect
		m_trackingMode = TRACKING_AUTOMATIC_CAMERA;
		m_shootingMode = SHOOTING_MANUAL;
		
		m_targetSelection = Target::TARGET_TOP;
		
		t_controlShoot = m_kinect->GetRawButton(1);
		t_controlPickup = m_kinect->GetRawButton(2);
		t_controlDump = false;
	break;
	
	case CONTROL_SINGLE_JOYSTICK:
		t_thetaMotorValue = -m_rightStick->GetX();
		t_phiMotorValue = sgn(-m_rightStick->GetY()) * -m_rightStick->GetY() * -m_rightStick->GetY() * PHI_MOTOR_MAX;
		t_speedMotorValue = ((-m_rightStick->GetZ() + 1.0) / 2);
		
		t_thetaSetpointChange = m_rightStick->GetX() * THETA_STEP;
		t_phiSetpointChange = sgn(-m_rightStick->GetY()) * -m_rightStick->GetY() * -m_rightStick->GetY() * PHI_STEP;
		m_speedSetpoint = ((-m_rightStick->GetZ() + 1.0) / 2) * SPEED_MAX;
		t_speedSetpointChange = 0.0;
		
		t_controlPickup = m_leftStick->GetRawButton(2);
		t_controlShoot = m_leftStick->GetRawButton(1);
		t_controlDump = m_leftStick->GetRawButton(3);
		
		m_targetSelection = Target::TARGET_TOP;
	break;
	
	case CONTROL_DOUBLE_JOYSTICK:
		t_thetaMotorValue = -m_rightStick->GetX();
		t_phiMotorValue = sgn(-m_rightStick->GetY()) * -m_rightStick->GetY() * -m_rightStick->GetY() * PHI_MOTOR_MAX;
		t_speedMotorValue = max(0.0, -m_leftStick->GetY());
		
		t_thetaSetpointChange = m_rightStick->GetX() * THETA_STEP;
		t_phiSetpointChange = sgn(-m_rightStick->GetY()) * -m_rightStick->GetY() * -m_rightStick->GetY() * PHI_STEP;
		t_speedSetpointChange = -m_leftStick->GetY() * SPEED_STEP;
		
		t_controlPickup = m_rightStick->GetRawButton(2);
		t_controlShoot = m_rightStick->GetRawButton(1);
		t_controlDump = m_rightStick->GetRawButton(3);
		
		if(m_leftStick->GetRawButton(2))
			m_targetSelection = Target::TARGET_BOTTOM;
		else if(m_leftStick->GetRawButton(3))
			m_targetSelection = Target::TARGET_TOP;
		else if(m_leftStick->GetRawButton(4))
			m_targetSelection = Target::TARGET_LEFT;
		else if(m_leftStick->GetRawButton(5))
			m_targetSelection = Target::TARGET_RIGHT;
		else
			m_targetSelection = Target::TARGET_NONE;
		
		// Fender Shots
		if(m_leftStick->GetRawButton(1))
		{
			m_targetSelection = Target::TARGET_BOTTOM;
			t_thetaSetpointChange = 0.0;
			m_phiSetpoint = FENDER_PHI;
			m_speedSetpoint = FENDER_SPEED;
		}
	break;
	
	default:
		t_thetaMotorValue = t_phiMotorValue = t_speedMotorValue = t_thetaSetpointChange = t_phiSetpointChange = t_speedSetpointChange = 0.0;
		t_controlPickup = t_controlShoot = t_controlDump = false;
		m_targetSelection = Target::TARGET_NONE;
	break; 
	}
	
	if(t_thetaSetpointChange < 1.6 && t_thetaSetpointChange > -1.6)
		t_thetaSetpointChange = 0.0;
	if(t_phiSetpointChange < 0.1 && t_phiSetpointChange > -0.1)
		t_phiSetpointChange = 0.0;
	if(t_speedSetpointChange < 36.0 && t_speedSetpointChange > -36.0)
		t_speedSetpointChange = 0.0;
	
	m_thetaSetpoint += t_thetaSetpointChange;
	m_phiSetpoint += t_phiSetpointChange;
	m_speedSetpoint += t_speedSetpointChange;
	
	if(m_thetaSetpoint < THETA_MIN) m_thetaSetpoint = THETA_MIN;
	if(m_thetaSetpoint > THETA_MAX) m_thetaSetpoint = THETA_MAX;
	if(m_phiSetpoint < 0.0) m_phiSetpoint = 0;
	if(m_phiSetpoint > PHI_MAX) m_phiSetpoint = PHI_MAX;
	if(m_speedSetpoint < 0.0) m_speedSetpoint = 0;
	if(m_speedSetpoint > SPEED_MAX) m_speedSetpoint = SPEED_MAX;
	
	if(t_controlPickup) m_beltSystem.Pickup();
	if(t_controlDump) m_beltSystem.Dump();
	
	switch(m_shootingMode)
	{
	case SHOOTING_AUTOMATIC:
		Shoot();
	break;
	
	case SHOOTING_CONFIDENCE:
		if(m_turret.GetConfidence() > m_confidenceThreshold && t_controlShoot) Shoot();
	break;
	
	case SHOOTING_MANUAL:
		if(t_controlShoot) Shoot();
	break;
	
	default:
	break;
	}
	
	switch(m_trackingMode)
	{
	case TRACKING_AUTONOMOUS:
		m_turret.UpdatePIDValues(0.0, AUTONOMOUS_PHI_SETPOINT, AUTONOMOUS_SPEED_SETPOINT);
//		m_turret.UpdateAutomatic(m_targetSelection, AUTONOMOUS_PHI_SETPOINT, AUTONOMOUS_SPEED_SETPOINT);
	break;
		
	case TRACKING_AUTOMATIC_CAMERA:
		if(m_targetSelection != Target::TARGET_NONE)
		{
			m_turret.UpdateAutomatic(m_targetSelection, m_phiSetpoint, m_speedSetpoint);
		}
		else
		{
			m_turret.UpdatePIDValues(t_thetaSetpointChange, m_phiSetpoint, m_speedSetpoint);
		}
	break;
	
	case TRACKING_MANUAL_PID:
		m_turret.UpdatePIDValues(t_thetaSetpointChange, m_phiSetpoint, m_speedSetpoint);
	break;
	
	case TRACKING_MANUAL:
		m_turret.UpdateMotorValues(t_thetaMotorValue, t_phiMotorValue, t_speedMotorValue);
	break;
	
	default:
	break;
	}
	
	m_beltSystem.Update();
}

void RobotPayload::SetControlMode(ControlMode _mode)
{
	m_controlMode = _mode;
}

void RobotPayload::SetTrackingMode(TrackingMode _mode)
{
	m_trackingMode = _mode;
}

void RobotPayload::SetShootingMode(ShootingMode _mode)
{
	m_shootingMode = _mode;
}

void RobotPayload::SetConfidenceThreshold(float _threshold)
{
	m_confidenceThreshold = _threshold;
}

void RobotPayload::Shoot()
{
	m_beltSystem.Shoot();
}

}
